/*
 * Planner.cpp
 *
 *  Created on: Aug 26, 2009
 *      Author: naveenoid
 */

#include "Planner.hpp"
#include "./../Common/Common.hpp"
#include <math.h>

using Octopus::MainLoop::Loop;
using Octopus::ToolKit::Log::Logger;
using ECL::Threads::Mutex;
using Octopus::Common::uWait;
using Octopus::Common::sWait;

namespace Octopus {

namespace Trajectory {


//static void initialise();
//void remove(void);
//
//void start(void);
//bool stop(void);
//
//bool finished(void);

Planner* Planner::planner;

Planner* Planner::getInstance(Loop *loop, Logger *log)
{
	if(planner==NULL)
	{
		planner = new Planner(loop, log);
		printf("New TrajectoryPlanner created\n");
	}
	return(planner);
}

void Planner::remove()
{

	printf("Removing Planner\n");

	while(!loopFinished)
	{
//		uWait(10000);
		sWait(0.1);
		printf("waiting for thread to finish\n");
	}

	delete(this);
}

void Planner::start()
{
	printf("Planner Thread begins\n");
	loopFinished = false;

//	while(loop->finished()) {
//		printf("TrajectoryPlanner Waiting for Control Loop to start\n");
//		wait(0.1);
//	}

	fixedPoint = (double)WAVE_AMPLITUDE;
	long double t = 0.0;
	double tempVal;
	while(true)
	{
		mutex.lock();
		if(!continueLoop)
		{
			mutex.unlock();
			break;
		}
		mutex.unlock();

		fixedPointMutex.lock();
		if(setPointsChanged==true)
		{
			printf("Setpoints to be changed in planner thread\n");

//			for(int i=0; i<MAX_NUM_OF_MOTOR ; i++) {
//				setPoints.at(i) = fixedPoint;
//
//				printf("Trajectory Planner changed Actuator %d to %f\n", i,fixedPoint);
//			}
//			loop->changeSetPoints(setPoints);
			setPointsChanged = false;
		}
//		else
//		{
//			printf("No setpoints given");
//		}
#if TRAJECTORY_TYPE == 1
		//fullwave
			setPoints.at(0) = fixedPoint/2 + ((fixedPoint/2)* sin(((double)WAVE_FREQUENCY) * t));
			setPoints.at(3) = fixedPoint/2 + ((fixedPoint/2)* sin((((double)WAVE_FREQUENCY) * t)+(double)PI));

			setPoints.at(1) = 0.001;
			setPoints.at(2) = 0.001;

		//half wave
//			tempVal = fixedPoint * sin((double)WAVE_FREQUENCY * t);
//			if(tempVal < 0.0) {
//				setPoints.at(0) = (-1)*  tempVal;
//				setPoints.at(3) = 0.01;
//			}
//			else {
//				setPoints.at(0) = 0.01;
//				setPoints.at(3) = tempVal;
//			}

//			setPoints.at(1) = 0.01;
//			setPoints.at(2) = 0.01;
#elif TRAJECTORY_TYPE == 2

			tempVal = fixedPoint * sin((double)WAVE_FREQUENCY * t);
			if(tempVal < 0.0) {
//				setPoints.at(0) = (-1)*  tempVal;
				setPoints.at(0) = 0.75*fixedPoint;
				setPoints.at(3) = 0.0;
			}
			else {
				setPoints.at(0) = 0.0;
//				setPoints.at(3) = tempVal;
				setPoints.at(3) = 0.75*fixedPoint;
			}

			setPoints.at(1) = 0.1;
			setPoints.at(2) = 0.1;
#endif
		fixedPointMutex.unlock();

//		if(t>1.0)	{
			loop->changeSetPoints(setPoints);
//		}
	//	sWait(0.1);
		uWait(5*(double)LOOP_TIME);

//		printf("In planner thread\n");

		t+=0.05;
//	Do something
	}
	loopFinished = true;

	printf("Planner thread Finished\n");

}

bool Planner::stop()
{
	mutex.lock();
	continueLoop = false;
	mutex.unlock();
	return(true);
}

bool Planner::finished()
{
	return(loopFinished);
}

bool Planner::setFixedPoint(double point)
{
	printf("Setting the new point in the Planner\n");
	fixedPointMutex.lock();
	fixedPoint = point;
	setPointsChanged = true;
	fixedPointMutex.unlock();
	return(true);
}

Planner::Planner(Loop * lop, Logger *log) : setPoints(MAX_NUM_OF_MOTOR,0){
	loop = lop;
	logger = log;

	loopFinished = true;
	continueLoop = true;

	setPointsChanged = false;
	fixedPoint = 0.0;
}

Planner::~Planner() {

}

}

}
